﻿#include "AxRobotOdometry.h"
#include "math\euler.h"
#include "utils.h"



//std::deque<RangeReading>  AxRobotOdometry::m_rangeDeque;
//QMutex  AxRobotOdometry::global_Mutex;
//RangeSensor* AxRobotOdometry::m_Laser=0;
//bool AxRobotOdometry::m_threadRunning=false;
//OrientedPoint AxRobotOdometry::m_truepos;
//bool AxRobotOdometry::stopped=true;
//SensorMap AxRobotOdometry::m_sensorMap;

AxRobotOdometry::AxRobotOdometry(void):flag(-1),linkFlag(0)
	,safedrive(true),grid(false)
	,mobileSim(true),m_Laser(0)
	,m_threadRunning(false),stopped(true)
	,laserReadingNo(0)
	,firstLaser(0),firstMoveCtrl(0),m_isGoalSet(false),misVFF(true)
	,queue_size_(5),message_count_(0),oldDistance(0)
{
}


AxRobotOdometry::~AxRobotOdometry(void)
{
}

void AxRobotOdometry::init()
{

}
void AxRobotOdometry:: stopLaser()
{
	laser.scanContinous(0);
	laser.stopMeas();
	laser.disconnect();
	global_Mutex.lock();
	flag=ROBOT_STOP;
	global_Mutex.unlock();
	msleep(100);
}

void AxRobotOdometry::startRobot()
{
	//viewer->clear_plot_data();
	global_Mutex.lock();
	is_pause=false;
	global_Mutex.unlock();
	start();
}

void AxRobotOdometry::run(void)
{
	Aria::init();
	//int argc=5; 
	//char *argv[6]; //argc是字符串个数，argv是字符串内容
	//argv[0]="./ArClientDemo.exe";
	//argv[1]="-host";                                                           
	//argv[2]=(LPSTR)(LPCTSTR)Global_IP; //Sim-192.168.244.128/ LX-213.111     
	//argv[3]="-port";                                                          
	//argv[4]="7272";
	//argv[5]=0;

	/*ArClientBase client;*/

	/* Aria components use this to get options off the command line: */
	//ArArgumentParser parser(&argc, argv);
	ArArgumentBuilder  *arg1=new ArArgumentBuilder();
	ArArgumentParser parser(arg1);

	/* This will be used to connect our client to the server, including
	* various bits of handshaking (e.g. sending a password, retrieving a list
	* of data requests and commands...)
	* It will get the hostname from the -host command line argument: */
	ArClientSimpleConnector clientConnector(&parser);

	parser.loadDefaultArguments();

	/* Check for -help, and unhandled arguments: */
	if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
	{
		Aria::logOptions();
		exit(0);
	}
  
	/* Connect our client object to the remote server: */	
	if (clientConnector.connectClient(&client)) 
		linkFlag=1;
	QString G_Warning;
	if (linkFlag!=1)
	{
		linkFlag=99;

		if (client.wasRejected())
			G_Warning = "Server rejected connection";
		else
			G_Warning = "Could not connect to server";

		ArUtil::sleep(3000);
		exit(1);
	}

	client.runAsync();

	ArKeyHandler keyHandler;
	Aria::setKeyHandler(&keyHandler);
	/*ArGlobalFunctor escapeCB(&escape);
	keyHandler.addKeyHandler(ArKeyHandler::ESCAPE, &escapeCB);*/

	InputHandler inputHandler(&client);

	inputHandler.safeDrive();
	

	ArFunctor1C<AxRobotOdometry, ArNetPacket *> robotInfoCB(this,&AxRobotOdometry::robotInfo);
	client.addHandler("update", &robotInfoCB);

	ArFunctor1C<AxRobotOdometry, ArNetPacket *> laserCurrentCB(this,&AxRobotOdometry::laserCurrent);
	client.addHandler("getSensorCurrent",&laserCurrentCB);

	ArFunctor1C<AxRobotOdometry, ArNetPacket *> getLocStateCB(this,&AxRobotOdometry::getLocState);
	client.addHandler("getLocState", &getLocStateCB);

	ArFunctor1C<AxRobotOdometry, ArNetPacket *> getMapCB(this,&AxRobotOdometry::getMap);
	client.addHandler("getMap", &getMapCB);

	ArFunctor1C<AxRobotOdometry, ArNetPacket *> getLocPointsCB(this,&AxRobotOdometry::getLocPoints);
	client.addHandler("getLocPoints",&getLocPointsCB);


	/* sending requests to server */
	//client.requestOnce("getMapName");
	//client.requestOnce("getGoals");
	//client.request("update",TimeInterval);
	//client.request("getLocState",TM-2);
	//client.request("pathPlannerStatus",TM-3);
	
	//-------- sim_S3Series_1 for mobileSim use only --------//
	ArNetPacket requestPacket;
	if(mobileSim==true) requestPacket.strToBuf("sim_lms1xx_1"); 
	//if(mobileSim==false) requestPacket.strToBuf("S3Series_1"); 
	//client.request("getSensorCurrent",TM-4,&requestPacket); 
	//client.request("getPath",TM*2-5);

	
	if(client.dataExists("popupClicked"))
	{
		ArNetPacket popupClick;
		popupClick.byte4ToBuf(0);
		popupClick.byteToBuf(1);
		client.requestOnce("popupClicked",&popupClick);
	}

	if(client.dataExists("autoDockDisable"))
	{
		client.requestOnce("autoDockDisable");
	}

	while(client.getRunningWithLock())
	{
		ArNetPacket pkt, pkt_goal, pkt_map;
		global_Mutex.lock();
		if (flag!=ROBOT_STOP)
		{
			//请求机器人状态信息
			client.requestOnce("update");
			//请求机器人激光传感器信息
			if(mobileSim==true)
			{
				client.requestOnce("getSensorCurrent",&requestPacket);	
			}
		}
		switch(flag)
		{
			case ROBOT_UP:inputHandler.up();break;  //if(GetAsyncKeyState(VK_LBUTTON)&0x8000)
			case ROBOT_DOWN:inputHandler.down();break;
			case ROBOT_LEFT:inputHandler.left();break;
			case ROBOT_RIGHT:inputHandler.right();break;
			case ROBOT_STOP:inputHandler.space();break;
			//case 5:client.requestOnce("stop");ArUtil::sleep(25);flag=0;break;  //inputHandler.doStop();
			case ROBOT_LOCALIZETOPOSE:
				{
					pkt.byte4ToBuf(m_Goal.getX());
					pkt.byte4ToBuf(m_Goal.getY());
					pkt.byte4ToBuf(m_Goal.getTh());
					client.requestOnce("axGotoPose",&pkt);
					flag=ROBOT_UNKNOW;
					break; // for mobileSim use ONLY
				}
			case ROBOT_GOTOHEADING:
				{
					//if (m_AjustTruePose)
					{
						pkt.byte4ToBuf(m_TruePose.getX());//m_TruePose在每次读取激光数据的时候更新
						pkt.byte4ToBuf(m_TruePose.getY());
						pkt.byte4ToBuf(m_TruePose.getTh());
					}
					pkt.byte4ToBuf(m_Goal.getX());
					pkt.byte4ToBuf(m_Goal.getY());
					pkt.byte4ToBuf(m_Goal.getTh());
					pkt.byte4ToBuf(control_signal.direction);
					client.requestOnce("axGotoDeltaHeading",&pkt);
					flag=ROBOT_UNKNOW;
					break; // for mobileSim use ONLY
				}
			case ROBOT_AUTOEXPLORER:
				{
					pkt.byte4ToBuf(m_TruePose.getX());//m_TruePose在每次读取激光数据的时候更新
					pkt.byte4ToBuf(m_TruePose.getY());
					pkt.byte4ToBuf(m_TruePose.getTh());
					pkt.byte4ToBuf(m_Goal.getX());
					pkt.byte4ToBuf(m_Goal.getY());
					pkt.byte4ToBuf(m_Goal.getTh());
					pkt.byte4ToBuf(control_signal.direction);
					client.requestOnce("axGotoDeltaHeading",&pkt);
					flag=ROBOT_UNKNOW;
					break; // for mobileSim use ONLY
				}
			case ROBOT_DISCONNECT:client.disconnect();flag=0;break; // disconnect from server
		}
		msleep(200);
		if(safedrive)//安全模式
		{
			inputHandler.safeDrive();
		} 
		else
		{
			inputHandler.unsafeDrive();
		}
		//inputHandler.sendInput(); // don't change this code sequence here!		
		global_Mutex.unlock();
		ArUtil::sleep(100); // set sleep time here to avoid use of too much CPU resource
	}
}

int AxRobotOdometry::openUrg(int port_)
{
	if (mobileSim)
	{
		return true;
	}
	laser.connect("169.254.39.84", 2112);
	if (!laser.isConnected())
	{
		std::cout << "connection fail\n" << std::endl;
		return 0;
	}

	std::cout << "Connected to laser\n" << std::endl;

	std::cout << "Loging in ..." << std::endl;
	laser.login();

	laser.stopMeas();

	std::cout << "Geting scan configuration ..." << ::std::endl;
	scanCfg c = laser.getScanCfg();

	std::cout << "Scanning Frequency : " << c.scaningFrequency / 100.0 << "Hz AngleResolution : " << c.angleResolution / 10000.0 << "deg " << std::endl;

	c.angleResolution = 2500;
	c.scaningFrequency = 2500;

	laser.setScanCfg(c);

	scanDataCfg cc;
	cc.deviceName = false;
	cc.encoder = 0;
	cc.outputChannel = 1;
	cc.remission = true;
	cc.resolution = 1;
	cc.position = false;
	cc.outputInterval = 1;

	laser.setScanDataCfg(cc);

	cfg = laser.getScanCfg();
	std::cout << "Scanning Frequency : " << cfg.scaningFrequency / 100.0 << "Hz AngleResolution : " << cfg.angleResolution / 10000.0 << "deg " << std::endl;

	
	if (cfg.angleResolution == 2500)
	{
		num_values = 1081;
	}
	else if (cfg.angleResolution == 5000)
	{
		num_values = 541;
	}
	else
	{
		std::cout << "Unsupported resolution." << std::endl;
		return 0;
	}
	int ret = 0;
	std::cout << "Start measurements ..." << std::endl;
	laser.startMeas();

	std::cout << "Wait for ready status ..." << std::endl;
	ret = 0;
	std::cout << "status : " << ret << std::endl;
	Sleep(1);
	while (ret != ready_for_measurement)
	{
		ret = laser.queryStatus();
		printf("Laser not ready. Retrying initialization.\n");
		//laser.disconnect();
	}
	std::cout << "Laser ready" << std::endl;
	laser.startDevice();
	std::cout << "Start continuous data transmission ..." << std::endl;
	laser.scanContinous(1);
	return ret;
}
void AxRobotOdometry::setSpeed(double flag_)
{
	global_Mutex.lock();
	deltaSpeed=5;
	global_Mutex.unlock();
}
void AxRobotOdometry::setFlag(int flag_)
{
	global_Mutex.lock();
	flag=flag_;
	global_Mutex.unlock();
}


void AxRobotOdometry::setSafeDrive(bool safe_)
{
	global_Mutex.lock();
	safedrive=safe_;
	global_Mutex.unlock();
}

void AxRobotOdometry::setTruePoseDifferent(double dx,double dy,double dtheta_)
{
	global_Mutex.lock();
	m_TruePoseDifferent.setX(dx);
	m_TruePoseDifferent.setY(dy);
	m_TruePoseDifferent.setThRad(dtheta_);
	global_Mutex.unlock();
}

void AxRobotOdometry::setTruePoseDifferent(Eigen::Matrix3d& dtheta_)
{
	global_Mutex.lock();
	m_TruePoseRelate=dtheta_;
	global_Mutex.unlock();
}



void AxRobotOdometry::setTruePoseScanMatch(OrientedPoint & tPose,OrientedPoint& odom)
{
	global_Mutex.lock();
	m_TruePoseScanMatch.x=tPose.x;
	m_TruePoseScanMatch.y=tPose.y;
	m_TruePoseScanMatch.theta=tPose.theta;
	m_OdomScanMatch.x=odom.x;
	m_OdomScanMatch.y=odom.y;
	m_OdomScanMatch.theta=odom.theta;
	global_Mutex.unlock();
}

//运动到鼠标点击的位置
void AxRobotOdometry::setPoseToGoto(double posex_,double posey_,double theta_)
{
	global_Mutex.lock();
	m_Goal.setX(posex_);
	m_Goal.setY(posey_);
	m_Goal.setThRad(theta_);
	m_GoalSetType=GOAL_SET_TYPE_MOUSE;
	m_isGoalSet=true;
	global_Mutex.unlock();
}

//运动到自主探索的目标点
void AxRobotOdometry::setPoseToExplorer(double posex_,double posey_,double theta_)
{
	global_Mutex.lock();
	m_Goal.setX(posex_);
	m_Goal.setY(posey_);
	m_Goal.setThRad(theta_);
	m_GoalSetType=GOAL_SET_TYPE_AUTOEXPLORER;
	m_isGoalSet=true;
	global_Mutex.unlock();
}
ArPose AxRobotOdometry::getPoseToExplorer()
{
	return m_Goal;
}
//计算象限角
static double GetQuadrantAngle(double x, double y)
{
	double theta =atan(y / x);
	if (x == 0 && y == 0) return 0;
	if (x > 0 && y == 0) return 0;
	if (x == 0 && y > 0) return M_PI / 2;
	if (x < 0 && y == 0) return M_PI;
	if (x == 0 && y < 0) return 3 *M_PI / 2;

	if (x > 0 && y > 0) return theta;
	if (x > 0 && y < 0) return M_PI * 2+ theta;
	if (x < 0 && y > 0) return theta + M_PI;
	if (x < 0 && y < 0) return theta + M_PI;
	return theta;
}

OIMO::Euler MatrixToEuler2(const Eigen::Matrix3d& mtx)
{
	const double* te = mtx.data();
	float x, y, z;
	if (te[3] > 0.998f) { // singularity at north pole
		y = std::atan2(te[2], te[8]);
		z = M_PI_2;
		x = 0;
	}
	else if (te[3] < -0.998f) { // singularity at south pole
		y = std::atan2(te[2], te[8]);
		z = -M_PI_2;
		x = 0;
	}
	else {
		y = std::atan2(-te[6], te[0]);
		x = std::atan2(-te[5], te[4]);
		z = std::asin(te[3]);
	}
	return OIMO::Euler(x, y, z);
}
//这里重要，线程间隔100ms读取里程计信息，回调该方法。
//获取里程计之后读取激光数据，以此方式实现里程计和激光数据的时间同步。
//使用该方法前需要先打开激光，连接机器人
void AxRobotOdometry::robotInfo(ArNetPacket* packet)
{
	/* Extract the data from the update packet. Its format is status and
	* mode (null-terminated strings), then 6 doubles for battery voltage, 
	* x position, y position and orientation (theta) (from odometry), current
	* translational velocity, and current rotational velocity. Translation is
	* always milimeters, rotation in degrees.
	*/
	double myX_1;
	double myY_1;
	double myTh_1;
	double myVel_1;
	double myRotVel_1;
	double myVoltage_1;
	char myStatus_1[256];
	char myMode_1[32];

	memset(myStatus_1, 0, sizeof(myStatus_1));
	memset(myMode_1, 0, sizeof(myMode_1));
	packet->bufToStr(myStatus_1, sizeof(myStatus_1));
	packet->bufToStr(myMode_1, sizeof(myMode_1));
	myVoltage_1 = ( (double) packet->bufToByte2() )/10.0;
	myX_1 = (double) packet->bufToByte4();
	myY_1 = (double) packet->bufToByte4();
	myTh_1 = (double) packet->bufToByte2();
	myVel_1 = (double) packet->bufToByte2();
	myRotVel_1 = (double) packet->bufToByte2();

	fflush(stdout);

	double num_readings = 1081;
	//int max_size = urg_state.max_size;
	long range[1080]={-1};
	double Ddata[1080*3]={0};
	int CaptureTimes = 1;

	/////////////////////////////////////////////////////////////////////
	double theta=myTh_1*M_PI/180;
	if (mobileSim==false)
	{
		double res=M_PI*3/2/1080;
		string sensorName="FLASER";
		laserReadingNo=1080;
		m_Laser=new RangeSensor("SICK-LMS1xx",1080, res, OrientedPoint(0,0,0), 0, 20);
		m_Laser->updateBeamsLookup();
		m_sensorMap.insert(make_pair(sensorName, m_Laser));

		const RangeSensor * rs=m_Laser;

		laser.getScanData(&data);
		RangeReading reading(rs, 1);
		reading.resize(rs->beams().size());
		
		for (int j = 0; j < laserReadingNo; j++)
		{
			double l = data.dist1[j];
			reading[j] = l / 1000;//单位为m
			std::cout << data.dist1[j] << ",";
			double beta = (-45.0 + j* cfg.angleResolution / 10000.0)*M_PI / 180.0;//ȷ
			double x = l*cos(beta) / 1000;
			double y = l*sin(beta) / 1000;
			double inte = data.dist2[j];
		}
			
		reading.setPose(OrientedPoint(myX_1/1000, myY_1/1000, theta));//存储的是弧度
		long currentSec=QDateTime ::currentMSecsSinceEpoch();
		if (firstLaser==0)
		{
			addReading(reading);
			lastSec=currentSec;
			firstLaser++;
		}
		if (currentSec-lastSec>=500)
		{
			addReading(reading);
			lastSec=currentSec;
		}
		odometry=OrientedPoint(myX_1/1000, myY_1/1000, theta);//存储的是弧度

		double	dTheta=odometry.theta-m_OdomScanMatch.theta;
		double	dTruePoseTheta=m_TruePoseScanMatch.theta;
		Eigen::Matrix3d matOdometry;
		matOdometry(0,0)=cos(odometry.theta);
		matOdometry(0,1)=-sin(odometry.theta);
		matOdometry(0,2)=odometry.x;
		matOdometry(1,0)=cos(odometry.theta);
		matOdometry(1,1)=sin(odometry.theta);
		matOdometry(1,2)=odometry.y;
		matOdometry(2,0)=0;
		matOdometry(2,1)=0;
		matOdometry(2,2)=1;
		Eigen::Matrix3d odom2=m_TruePoseRelate*matOdometry;

		Eigen::Matrix3d matodom2=Eigen::Matrix3d::Identity();
		matodom2(0,0)=odom2(0,0);
		matodom2(0,1)=odom2(0,1);
		matodom2(0,2)=0;
		matodom2(1,0)=odom2(1,0);
		matodom2(1,1)=odom2(1,1);
		matodom2(1,2)=0;
		matodom2(2,0)=0;
		matodom2(2,1)=0;
		matodom2(2,2)=1;
		m_TruePose.setX(odom2(0,2)*1000);
		m_TruePose.setY(odom2(1,2)*1000);
		double fixedZthelta=dTheta+dTruePoseTheta;

		m_TruePose.setThRad(fixedZthelta);
		//抛出消息，进行绘制实时的激光和里程计
		emit LaserReceived(&m_TruePose);

		double deltax=(m_Goal.getX()-m_TruePose.getX())/1000;
		double deltay=(m_Goal.getY()-m_TruePose.getY())/1000;
		double distance=sqrt(deltax*deltax+deltay*deltay);
		oldDistance=distance;
		bool flag4going=false;
		if (m_GoalSetType==GOAL_SET_TYPE_AUTOEXPLORER)
		{
			double targetTheta=atan2(deltay,deltax);//弧度角（-PI，PI）
			bool tmp= distance<3.0 && abs(targetTheta-m_TruePose.getThRad())<M_PI/3;
			
			//如果机器人卡死了，强制寻找新的目标点
			//if (oldDistance-distance<0.2)
			//{
			//}
			//if (false)//卡死了
			//{
			//	m_isGoalSet=false;
			//	emit goalDone();//线程抛出消息，接收完毕
			//}
			flag4going=(m_isGoalSet==true && !tmp);
		}
		else if (m_GoalSetType==GOAL_SET_TYPE_MOUSE)
		{
			flag4going=(m_isGoalSet==true && distance>0.5);
		}
		if (flag4going)
		{
			if (misVFF)//是否使用虚拟力场法
			{
				vector<float> obstracles;

				double interval=(270.0/laserReadingNo);
				int fillnum=int(360.0/interval);

				double direction= m_TruePose.getThRad();//简单校正里程计后的方向角
				double ang = -180 + 0.5*interval;
				for (int i=0;i< fillnum;i++,ang+=interval)
				{
					if (ang<-135 && ang>=135)
					{
						obstracles.push_back(5.0);//限定激光扫描盲区为5m
					}
					else if (ang>= -135)
					{
						int num=(ang+135)/interval;//注意此处，从-180度开始逆时针存储数据
						if (num>0 && num<laserReadingNo)
						{
							float val=reading[num];
							if (val > 5.0)
							{
								obstracles.push_back(5.0);//激光数据范围进行限定
							}
							else if (val<= 0.105)//无效值，小于105mm，限定激光数据为5m
							{
								obstracles.push_back(5.0);//激光数据范围进行限定
							}
							else
							{
								obstracles.push_back(val);
							}
						}				
					}
				}
				double desiredDirection=0,desiredSpeed=200;
				double lasertargetX= (m_Goal.getX()-m_TruePose.getX())/1000;
				double lasertargetY= (m_Goal.getY()-m_TruePose.getY())/1000;
				//需要增加归一化过程，desiredDirection为弧度角
				navigate(GMapping::Point(lasertargetX,lasertargetY),obstracles,direction,2,12.0,1.0,desiredDirection,desiredSpeed);

				while (desiredDirection < -M_PI || desiredDirection > M_PI)
				{
					if (desiredDirection < -M_PI)
						desiredDirection += M_PI*2;
					else
						desiredDirection -= M_PI*2;
				}
				desiredDirection=desiredDirection;//+direction;
				long currentSec=QDateTime ::currentMSecsSinceEpoch();
				if (firstMoveCtrl==0)
				{
					control_signal.direction = desiredDirection*180/M_PI;
					this->flag=ROBOT_GOTOHEADING;
					lastSecMoveControl=currentSec;
					firstMoveCtrl++;
				}
				if (currentSec-lastSecMoveControl>=1000)
				{
					control_signal.direction = desiredDirection*180/M_PI;
					this->flag=ROBOT_GOTOHEADING;
					lastSecMoveControl=currentSec;
				}
				printf("\nNext direction: %d [degrees]\n", control_signal.direction);
			}
			else
			{
				int position_x, position_y;
				grid_t * certainty_grid= NULL;
				hist_t * polar_histogram= NULL;
				/*control_signal_t control_signal;*/
				certainty_grid = grid_init(61, 0.05);
				polar_histogram = hist_init(5, 10, 100, 2);

				if (certainty_grid == NULL) return;
				if (certainty_grid->cells == NULL) return;
				if (polar_histogram == NULL) return;
				if (polar_histogram->densities == NULL) return;

				position_x = position_y = (certainty_grid->dimension + 1) / 2;

				for(int i=0;i<laserReadingNo;i++)
				{ 
					double direction = -135.0+(double(270*i)/laserReadingNo);
					direction+= odometry.theta * 180 / M_PI;
					grid_update(certainty_grid, position_x, position_y, reading[i] ,direction);
				}
				//QPixmap* m_costmap= new QPixmap(500,500);
				QImage *m_costmap=new QImage(61,61,QImage::Format_ARGB32); 
				m_costmap->fill(Qt::white);
				QPainter painter(m_costmap);

				for(int indx=0; indx<m_costmap->width(); indx++)
				{
					for(int indy=0; indy<m_costmap->height(); indy++)
					{
						//注意，这里为图像的左上角点
						int v=certainty_grid->cells[indx+(m_costmap->width()-1-indy)*m_costmap->width()];
						int grayValue=v;
						if (v>0)
						{
							painter.setPen(QColor(grayValue, grayValue, grayValue));
							painter.drawPoint(indx,indy);
						}		
					}
				}
				char name[100];
				sprintf(name,"dump-%05d.png",firstLaser);
				cout << " Writing " << name <<" ..." << flush;
				bool rv=m_costmap->save(name,"PNG");

				hist_update(polar_histogram, certainty_grid);
				FILE* t_file=fopen("d:\\test.txt","w");
				if (t_file)
				{	
					for (int i = 0; i < polar_histogram->sectors; ++i) 
					{
						int c =polar_histogram->densities[i];
						fprintf(t_file,"%d ",c);
					}
					fprintf(t_file,"\n");
					fclose(t_file);
				}

				double obj_dir=GetQuadrantAngle(m_Goal.getX()/1000-odometry.x,(m_Goal.getY()/1000-odometry.y))*180/M_PI;

				control_signal.direction = calculate_direction(polar_histogram,obj_dir );
				this->flag=ROBOT_GOTOHEADING;
				printf("\nNext direction: %d [degrees]\n", control_signal.direction);

				free(certainty_grid);
				free(polar_histogram);

				certainty_grid = NULL;
				polar_histogram = NULL;
			}
		}
		else
		{
			m_isGoalSet=false;
			//发送消息，已经到达目标。
			emit goalDone();//线程抛出消息，接收完毕
		}	
	}
	else
	{
		odometry=OrientedPoint(myX_1/1000, myY_1/1000, theta);//存储的是弧度
	}
}



void AxRobotOdometry::laserCurrent(ArNetPacket* packet)
{
	//using namespace std;
	char sensorName[50];
	int laserX=0,laserY=0;
	std::ofstream write;
	memset(sensorName, 0, sizeof(sensorName));
	memset(laserReadingX, 0, sizeof(laserReadingX));
	memset(laserReadingY, 0, sizeof(laserReadingY));

	laserReadingNo = packet->bufToByte2();   // Number of laser data with X & Y
	packet->bufToStr(sensorName, sizeof(sensorName)); // Sensor's name

	for(int i=0;i<laserReadingNo;i++)
	{ 
		laserX = packet->bufToByte4();  // X coordinate for laser reading
		laserY = packet->bufToByte4();  // Y coordinate for laser reading

		laserReadingX[i] = laserX;
		laserReadingY[i] = laserY;
	}
	fflush(stdout);
	if (mobileSim==true)
	{
		double res=M_PI*3/2/(laserReadingNo-1);
		string sensorName="FLASER";
		m_Laser=new RangeSensor("lms1xx",laserReadingNo, res, OrientedPoint(0,0,0), 0, 20);
		m_Laser->updateBeamsLookup();
		m_sensorMap.insert(make_pair(sensorName, m_Laser));//目前这种构造方法有问题！！

		const RangeSensor * rs=m_Laser;
		RangeReading reading(rs, 1);
		reading.resize(rs->beams().size());
		int beamsize=rs->beams().size();
		if (laserReadingNo==beamsize)
		{
			for(int i=0;i<laserReadingNo;i++)
			{ 
				double x=laserReadingX[i];
				double y=laserReadingY[i];
				double rx =odometry.x;
				double ry =odometry.y;
				double dx = x/1000 - rx;		
				double dy = y/1000 - ry;
				double dist2 = dx*dx + dy*dy;
				reading[i]=sqrt(dist2);
			}
		}
		else if (laserReadingNo<beamsize)
		{
			for(int i=0;i<beamsize;i++)
			{ 
				if (i<laserReadingNo)
				{
					double x=laserReadingX[i];
					double y=laserReadingY[i];
					double rx =odometry.x;
					double ry =odometry.y;
					double dx = x/1000 - rx;		
					double dy = y/1000 - ry;
					double dist2 = dx*dx + dy*dy;
					reading[i]=sqrt(dist2);
				}
				else
				{
					reading[i]=-1;
				}
			}
		}
		else if (laserReadingNo > beamsize)
		{
			for(int i=0;i< beamsize;i++)
			{ 
				double x=laserReadingX[i];
				double y=laserReadingY[i];
				double rx =odometry.x;
				double ry =odometry.y;
				double dx = x/1000 - rx;		
				double dy = y/1000 - ry;
				double dist2 = dx*dx + dy*dy;
				reading[i]=sqrt(dist2);
			}
		}

		/*Eigen::Matrix3d matDeltaOdometry;
		double	dTheta=odometry.theta-m_OdomScanMatch.theta;
		matDeltaOdometry(0,0)=cos(dTheta);
		matDeltaOdometry(0,1)=-sin(dTheta);
		matDeltaOdometry(0,2)=odometry.x-m_OdomScanMatch.x;
		matDeltaOdometry(1,0)=sin(dTheta);
		matDeltaOdometry(1,1)=cos(dTheta);
		matDeltaOdometry(1,2)=odometry.y-m_OdomScanMatch.y;
		matDeltaOdometry(2,0)=0;
		matDeltaOdometry(2,1)=0;
		matDeltaOdometry(2,2)=1;

		Eigen::Matrix3d matTruePose;
		double	dTruePoseTheta=m_TruePoseScanMatch.theta;
		matTruePose(0,0)=cos(dTruePoseTheta);
		matTruePose(0,1)=-sin(dTruePoseTheta);
		matTruePose(0,2)=m_TruePoseScanMatch.x;
		matTruePose(1,0)=sin(dTruePoseTheta);
		matTruePose(1,1)=cos(dTruePoseTheta);
		matTruePose(1,2)=m_TruePoseScanMatch.y;
		matTruePose(2,0)=0;
		matTruePose(2,1)=0;
		matTruePose(2,2)=1;
		Eigen::Matrix3d odom2=matTruePose*matDeltaOdometry;*/
		double	dTheta=odometry.theta-m_OdomScanMatch.theta;
		double	dTruePoseTheta=m_TruePoseScanMatch.theta;
		Eigen::Matrix3d matOdometry;
		matOdometry(0,0)=cos(odometry.theta);
		matOdometry(0,1)=-sin(odometry.theta);
		matOdometry(0,2)=odometry.x;
		matOdometry(1,0)=cos(odometry.theta);
		matOdometry(1,1)=sin(odometry.theta);
		matOdometry(1,2)=odometry.y;
		matOdometry(2,0)=0;
		matOdometry(2,1)=0;
		matOdometry(2,2)=1;
		Eigen::Matrix3d odom2=m_TruePoseRelate*matOdometry;

		Eigen::Matrix3d matodom2=Eigen::Matrix3d::Identity();
		matodom2(0,0)=odom2(0,0);
		matodom2(0,1)=odom2(0,1);
		matodom2(0,2)=0;
		matodom2(1,0)=odom2(1,0);
		matodom2(1,1)=odom2(1,1);
		matodom2(1,2)=0;
		matodom2(2,0)=0;
		matodom2(2,1)=0;
		matodom2(2,2)=1;
		m_TruePose.setX(odom2(0,2)*1000);
		m_TruePose.setY(odom2(1,2)*1000);
		double fixedZthelta=dTheta+dTruePoseTheta;

		m_TruePose.setThRad(fixedZthelta);

		//抛出消息，进行绘制实时的激光和里程计
		emit LaserReceived(&m_TruePose);

		double deltax=(m_Goal.getX()-m_TruePose.getX())/1000;
		double deltay=(m_Goal.getY()-m_TruePose.getY())/1000;
		double distance=sqrt(deltax*deltax+deltay*deltay);
		bool flag4going=false;
		if (m_GoalSetType==GOAL_SET_TYPE_AUTOEXPLORER)
		{
			double targetTheta=atan2(deltay,deltax);//弧度角（-PI，PI）
			bool tmp= distance<3.0 && abs(targetTheta-m_TruePose.getThRad())<M_PI/3;
			flag4going=(m_isGoalSet==true && !tmp);
		}
		else if (m_GoalSetType==GOAL_SET_TYPE_MOUSE)
		{
			flag4going=(m_isGoalSet==true && distance>0.5);
		}
		if (flag4going)
		{
			if (misVFF)//是否使用虚拟力场法
			{
				vector<float> obstracles;

				double interval=(270.0/laserReadingNo);
				int fillnum=int(360.0/interval);
				
				double direction= m_TruePose.getThRad();//简单校正里程计后的方向角
				double ang = -180 + 0.5*interval;
				for (int i=0;i< fillnum;i++,ang+=interval)
				{
					if (ang<-135 && ang>=135)
					{
						obstracles.push_back(5.0);//限定激光扫描盲区为5m
					}
					else if (ang>= -135)
					{
						int num=(ang+135)/interval;//注意此处，从-180度开始逆时针存储数据
						if (num>0 && num<laserReadingNo)
						{
							float val=reading[num];
							if (val>5.0)//限定激光数据为5m
							{
								obstracles.push_back(5.0);//激光数据范围进行限定
							}
							else if (val<= 0.005)//无效值，小于5mm，限定激光数据为5m
							{
								obstracles.push_back(5.0);//激光数据范围进行限定
							}
							else
							{
								obstracles.push_back(val);
							}
						}				
					}
				}
				double desiredDirection=0,desiredSpeed=200;
				double lasertargetX= (m_Goal.getX()-m_TruePose.getX())/1000;
				double lasertargetY= (m_Goal.getY()-m_TruePose.getY())/1000;
				//需要增加归一化过程，desiredDirection为弧度角
				navigate(GMapping::Point(lasertargetX,lasertargetY),obstracles,direction,2,2.0,1.0,desiredDirection,desiredSpeed);

				while (desiredDirection < -M_PI || desiredDirection > M_PI)
				{
					if (desiredDirection < -M_PI)
						desiredDirection += M_PI*2;
					else
						desiredDirection -= M_PI*2;
				}
				desiredDirection=desiredDirection;//+direction;
				long currentSec=QDateTime ::currentMSecsSinceEpoch();
				if (firstMoveCtrl==0)
				{
					control_signal.direction = desiredDirection*180/M_PI;
					this->flag=ROBOT_GOTOHEADING;
					lastSecMoveControl=currentSec;
					firstMoveCtrl++;
				}
				if (currentSec-lastSecMoveControl>=500)
				{
					control_signal.direction = desiredDirection*180/M_PI;
					this->flag=ROBOT_GOTOHEADING;
					lastSecMoveControl=currentSec;
				}
				printf("\nNext direction: %d [degrees]\n", control_signal.direction);
			}
			else
			{
				int position_x, position_y;
				grid_t * certainty_grid= NULL;
				hist_t * polar_histogram= NULL;
				/*control_signal_t control_signal;*/
				certainty_grid = grid_init(61, 0.05);
				polar_histogram = hist_init(5, 10, 100, 2);

				if (certainty_grid == NULL) return;
				if (certainty_grid->cells == NULL) return;
				if (polar_histogram == NULL) return;
				if (polar_histogram->densities == NULL) return;

				position_x = position_y = (certainty_grid->dimension + 1) / 2;

				for(int i=0;i<laserReadingNo;i++)
				{ 
					double direction = -135.0+(double(270*i)/laserReadingNo);
					direction+= odometry.theta * 180 / M_PI;
					grid_update(certainty_grid, position_x, position_y, reading[i] ,direction);
				}
				//QPixmap* m_costmap= new QPixmap(500,500);
				QImage *m_costmap=new QImage(61,61,QImage::Format_ARGB32); 
				m_costmap->fill(Qt::white);
				QPainter painter(m_costmap);

				for(int indx=0; indx<m_costmap->width(); indx++)
				{
					for(int indy=0; indy<m_costmap->height(); indy++)
					{
						//注意，这里为图像的左上角点
						int v=certainty_grid->cells[indx+(m_costmap->width()-1-indy)*m_costmap->width()];
						int grayValue=v;
						if (v>0)
						{
							painter.setPen(QColor(grayValue, grayValue, grayValue));
							painter.drawPoint(indx,indy);
						}		
					}
				}
				char name[100];
				sprintf(name,"dump-%05d.png",firstLaser);
				cout << " Writing " << name <<" ..." << flush;
				bool rv=m_costmap->save(name,"PNG");

				hist_update(polar_histogram, certainty_grid);
				FILE* t_file=fopen("d:\\test.txt","w");
				if (t_file)
				{	
					for (int i = 0; i < polar_histogram->sectors; ++i) 
					{
						int c =polar_histogram->densities[i];
						fprintf(t_file,"%d ",c);
					}
					fprintf(t_file,"\n");
					fclose(t_file);
				}

				double obj_dir=GetQuadrantAngle(m_Goal.getX()/1000-odometry.x,(m_Goal.getY()/1000-odometry.y))*180/M_PI;
				control_signal.direction = calculate_direction(polar_histogram,obj_dir );
				this->flag=ROBOT_GOTOHEADING;
				printf("\nNext direction: %d [degrees]\n", control_signal.direction);

				free(certainty_grid);
				free(polar_histogram);

				certainty_grid = NULL;
				polar_histogram = NULL;
			}
		}
		else
		{
			m_isGoalSet=false;
			//发送消息，已经到达目标。
			emit goalDone();//线程抛出消息，接收完毕
		}

		reading.setPose(OrientedPoint(odometry.x, odometry.y, odometry.theta));
		
		//QDateTime  now=QDateTime ::currentMSecsSinceEpoch();
		long currentSec=QDateTime ::currentMSecsSinceEpoch();//long(())*1000+now.msec();
		if (firstLaser==0)
		{
			addReading(reading);
			lastSec=currentSec;
			firstLaser++;
		}
		if (currentSec-lastSec>=1000)
		{
			addReading(reading);
			lastSec=currentSec;
		}
		
	}
}

bool AxRobotOdometry::getReading(RangeReading& reading)
{
	bool present=false;
	
	global_Mutex.lock();
	if (!m_rangeDeque.empty())
	{
		reading=m_rangeDeque.front();
		m_rangeDeque.pop_front();
		--message_count_;
		present=true;
	}
	int sval;
	
	global_Mutex.unlock();
	return present;
}

void AxRobotOdometry::addReading(RangeReading& reading)
{
	global_Mutex.lock();
	//增加队列大小限制？？
	if (queue_size_ != 0 && message_count_ + 1 > queue_size_)
	{
		m_rangeDeque.pop_front();
		--message_count_;
	}
	m_rangeDeque.push_back(reading);
	++message_count_;
	global_Mutex.unlock();

}

SensorMap& AxRobotOdometry::getSensorMap()
{
	return m_sensorMap;
}